双臂/外部轴关节同步避障接口使用说明

[TOC]

1. 功能概述

关节同步避障用于两个运动系统分别沿各自在关节空间生成的无碰路径同步运动的场景。两个系统由同一个 Planner 联合规划，规划状态为主、从关节的组合：

state = [q_master, q_slave]

规划过程中会同时检查主侧、从侧、主从之间以及机器人与环境之间的碰撞，并为主、从系统分别生成关节空间无碰路径。两侧路径点通过同一组同步路点一一对应，每个同步路点同时包含 joint_m 和 joint_s。支持的典型组合如下：

本文中的“主”和“从”表示同一个规划器内的角色及数据排列顺序，不表示控制器主从站关系。

1.1 Planner 组织方式

本文介绍的主从关节同步避障使用一个包含 MASTER 和 SLAVE 的 Planner：

auto planner_h = scene->rlCreateRobotPlanner(
    {{model_master_h, state_master_h, PlannerRole::MASTER},
     {model_slave_h,  state_slave_h,  PlannerRole::SLAVE}},
    "joint_sync_avoidance_planner");

系统同时支持多 Planner 避障：可以为不同机器人分别创建 Planner，并由各 Planner 分别调用 tpGenerateCollisionFreePoints() 生成各自的关节空间无碰路径。多 Planner 的避障及同步执行属于另一种规划组织方式，不在本文的主从关节同步避障范畴内，相关用法参见多机同步规划说明。

1.2 规划与执行流程

创建同一 Scene 下的主、从 Model 和 State
                    ↓
创建一个包含 MASTER + SLAVE 的 Planner
                    ↓
构造 JOINT 路点（每点同时填写 joint_m 和 joint_s）
                    ↓
Planner::tpGenerateCollisionFreePoints
                    ↓
得到主从同步无碰路点（joint_m + joint_s）
                    ↓
Planner::tpAddPoints 进行 B 样条速度规划
                    ↓
Task::tskUpdateCycle 同周期输出主、从轨迹点

碰撞检测在 tpGenerateCollisionFreePoints() 的路径搜索阶段完成。执行阶段的 tskUpdateCycle() 负责从主、从两条关节空间无碰路径中同步取点，不用于替代避障规划。

2. 使用条件与输入要求

2.1 模型和场景

1. 主、从模型必须创建在同一个 Scene 中。
2. URDF/SRDF 中应包含正确的连杆碰撞几何、自碰撞配置和关节限位。
3. 必须通过 Model::mdlSetBasePoseInWorld() 设置两套运动系统在同一世界坐标系下的实际安装位姿，否则主从互碰检测使用的相对位置不正确。
4. 起点、终点以及用户指定的途经点必须本身无碰撞且不超关节限位。

2.2 路点

Planner::tpGenerateCollisionFreePoints() 至少需要两个输入路点。关节同步避障的每个输入路点均需满足：

point.type    = DescribeSpace::JOINT;
point.joint_m = q_master;  // 维数等于主侧 DOF
point.joint_s = q_slave;   // 维数等于从侧 DOF

任一路点未填写 joint_s、关节维数错误或不是关节空间路点，均不能按关节同步避障处理。

2.3 路径和运动属性

PathProperty path_property;
path_property.describe_space = DescribeSpace::JOINT;
path_property.cur_prop.type = CurveType::BSPLINE;

ObstacleAvoidanceParameters avoidance;
avoidance.avoid_type = ObstacleAvoidanceAlgorithm::RRTCONNECT;
avoidance.maxDistance = 0.15;
avoidance.simplify = true;

• avoid_type：推荐默认使用 RRTCONNECT；也可选择 RRT 或 LBKPIECE。
• maxDistance：搜索树单次扩展的最大步长。数值过大可能降低窄通道搜索质量，过小会增加规划耗时，应结合关节量纲和场景调试。
• simplify：在保持无碰的前提下简化路径，通常保持为 true。
• MoveProperty::maxV/maxA/maxJ：按 主侧关节在前、从侧关节在后 的顺序填写，总维数为 master_dof + slave_dof。

3. 外部轴 + 机械臂关节同步避障 Demo

下面以 1 轴导轨 + 6 轴机械臂为例，展示外部轴与机械臂分别沿各自的关节空间无碰路径同步运动，并绕开环境障碍物。模型路径、关节值、障碍物尺寸和安装位姿仅为调用示例，部署时必须替换为现场配置。

#include <aral/robot_scene.hpp>
#include <aral/shapes.hpp>
#include <iostream>
#include <vector>

using namespace ARAL::interface;

int main()
{
    constexpr double cycle = 0.005;

    // 1. 创建场景、外部轴模型和机械臂模型
    auto scene = static_cast<Scene*>(CreateARALScene("external_axis_arm_avoidance"));
    if (!scene) {
        std::cerr << "Failed to create ARAL scene" << std::endl;
        return -1;
    }

    auto external_model_h = scene->rlCreateRobotModel(
        "track", "/path/to/external_axis_model/");
    auto arm_model_h = scene->rlCreateRobotModel(
        "arm", "/path/to/robot_model/");
    if (external_model_h < 0 || arm_model_h < 0) {
        return -1;
    }

    auto external_model = scene->rlGetRobotModel(external_model_h);
    auto arm_model = scene->rlGetRobotModel(arm_model_h);
    if (!external_model || !arm_model) {
        return -1;
    }

    // 外部轴与机械臂必须位于同一个世界坐标系；以下安装位姿仅为示例。
    external_model->mdlSetBasePoseInWorld(
        {0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
    arm_model->mdlSetBasePoseInWorld(
        {0.0, 0.0, 0.10, 0.0, 0.0, 0.0});

    // 2. 通过 Utility 将环境障碍物注册到当前 Scene 的 World 中。
    auto utility = scene->rlGetUtility();
    if (!utility) {
        return -1;
    }

    auto obstacle = std::make_shared<geometric_shapes::Box>(0.25, 0.25, 0.40);
    RLPose obstacle_pose = {0.50, 0.15, 0.45, 0.0, 0.0, 0.0};
    RLPose obstacle_origin = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    int ret = utility->utlSetWorldObject("obstacle_box", obstacle_pose, {obstacle}, {obstacle_origin});
    if (ret < 0) {
        std::cerr << "Failed to add obstacle: " << ret << std::endl;
        return ret;
    }

    // 3. 创建外部轴与机械臂状态
    auto external_state_h = scene->rlCreateRobotState(
        external_model_h, "external_axis_state");
    auto arm_state_h = scene->rlCreateRobotState(
        arm_model_h, "arm_state");
    if (external_state_h < 0 || arm_state_h < 0) {
        std::cerr << "Failed to create external-axis or arm state"
                  << std::endl;
        return -1;
    }

    auto external_state = scene->rlGetRobotState(external_state_h);
    auto arm_state = scene->rlGetRobotState(arm_state_h);
    if (!external_state || !arm_state) {
        std::cerr << "Failed to get external-axis or arm state"
                  << std::endl;
        return -1;
    }

    // 4. 为两个 State 分别创建 Controller。
    auto external_controller = scene->rlCreateRobotController(
        external_state_h, "external_axis_controller");
    auto arm_controller = scene->rlCreateRobotController(
        arm_state_h, "arm_controller");
    if (!external_controller || !arm_controller) {
        std::cerr << "Failed to create external-axis or arm controller"
                  << std::endl;
        return -1;
    }

    // 5. 创建管理主从系统的 Planner 和 Task。
    auto planner_h = scene->rlCreateRobotPlanner(
        {{external_model_h, external_state_h, PlannerRole::MASTER},
         {arm_model_h,      arm_state_h,      PlannerRole::SLAVE}},
        "external_axis_arm_avoidance_planner");
    auto planner = scene->rlGetRobotPlanner(planner_h);
    if (!planner) {
        return -1;
    }

    auto task = scene->rlCreateRobotTask(
        {planner_h}, "external_axis_arm_avoidance_task");
    if (!task) {
        return -1;
    }

    // 6. 初始化外部轴与机械臂状态
    RLJntArray q_external_start = {0.30};
    RLJntArray q_external_goal  = {1.00};
    RLJntArray q_arm_start = {0.20,  0.10, 0.80, 0.00, 1.20, 0.00};
    RLJntArray q_arm_goal  = {0.60, -0.10, 1.10, 0.20, 1.00, 0.30};
    RLJntArray qd_external_start = {0.0};
    RLJntArray qdd_external_start = {0.0};
    RLJntArray qd_arm_start = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    RLJntArray qdd_arm_start = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    ToolWorkpiece tool_workpiece;

    ret = external_state->rsInitiateRobotState(
        q_external_start, qd_external_start, qdd_external_start,
        tool_workpiece);
    if (ret < 0) return ret;
    ret = arm_state->rsInitiateRobotState(
        q_arm_start, qd_arm_start, qdd_arm_start, tool_workpiece);
    if (ret < 0) return ret;

    task->tskSetCycle(cycle);
    task->tskSetTaskState(TaskState::IDEL);

    // 7. 构造同步避障起终点；每个路点必须同时提供 joint_m 和 joint_s。
    PathPoint start;
    start.id = 0;
    start.type = DescribeSpace::JOINT;
    start.joint_m = q_external_start;
    start.joint_s = q_arm_start;
    start.t_w = tool_workpiece;

    PathPoint goal;
    goal.id = 1;
    goal.type = DescribeSpace::JOINT;
    goal.joint_m = q_external_goal;
    goal.joint_s = q_arm_goal;
    goal.t_w = tool_workpiece;

    PathProperty path_property;
    path_property.describe_space = DescribeSpace::JOINT;
    path_property.cur_prop.type = CurveType::BSPLINE;

    ObstacleAvoidanceParameters avoidance;
    avoidance.avoid_type = ObstacleAvoidanceAlgorithm::RRTCONNECT; //默认RRTCONNECT, 可以不显式设置
    avoidance.maxDistance = 0.15;
    avoidance.simplify = true;

    // 8. 在主从组合关节空间中搜索无碰路径。
    std::vector<PathPoint> collision_free_points;
    ret = planner->tpGenerateCollisionFreePoints(
        {start, goal}, path_property, avoidance, collision_free_points);
    if (ret < 0 || collision_free_points.size() < 2) {
        std::cerr << "Failed to generate collision-free path: " << ret << std::endl;
        return ret < 0 ? ret : -1;
    }

    // 可选：检查每个同步路点是否同时携带 1 轴外部轴和 6 轴机械臂关节。
    for (const auto& point : collision_free_points) {
        if (point.joint_m.size() != 1 || point.joint_s.size() != 6) {
            std::cerr << "Invalid external-axis/arm collision-free point"
                      << std::endl;
            return -1;
        }
    }

    // 9. 对无碰路点进行 B 样条轨迹规划。
    MoveProperty move_property;
    // 约束顺序：[外部轴关节, 机械臂关节 1 ... 机械臂关节 6]。
    move_property.maxV = {0.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
    move_property.maxA = {1.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0};
    move_property.maxJ = {5.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0};

    ret = planner->tpAddPoints(
        collision_free_points, path_property, move_property);
    if (ret < 0) {
        std::cerr << "Failed to add collision-free points: " << ret << std::endl;
        return ret;
    }

    // 10. 同周期取参考轨迹，通过 Controller 计算并下发关节命令。
    JointCommand external_cmd;
    external_cmd.q = q_external_start;
    external_cmd.qd = qd_external_start;
    external_cmd.qdd = qdd_external_start;

    JointCommand arm_cmd;
    arm_cmd.q = q_arm_start;
    arm_cmd.qd = qd_arm_start;
    arm_cmd.qdd = qdd_arm_start;

    Array2d ik_eps{{1e-6, 1e-5}};
    Array2d tune_eps{{10.0, 10.0}};

    while (true) {
        std::vector<TrajectoryPoint> points;
        PlannerHandle failed_planner;
        ret = task->tskUpdateCycle(cycle, points, failed_planner);

        if ((ret == 0 || ret == 40001 || ret == 40002) && points.size() == 2) {
            // 按模型句柄查找对应的 State、Controller 和关节命令。
            for (const auto& point : points) {
                StatePtr state;
                ControllerPtr controller;
                JointCommand* cmd = nullptr;

                if (point.id == external_model_h) {
                    state = external_state;
                    controller = external_controller;
                    cmd = &external_cmd;
                } else if (point.id == arm_model_h) {
                    state = arm_state;
                    controller = arm_controller;
                    cmd = &arm_cmd;
                } else {
                    std::cerr << "Unknown model handle: " << point.id
                              << std::endl;
                    return -1;
                }

                // 实机中先读取当前硬件反馈并调用 rsUpdateJointState()；

                // 将 Task 当前周期输出写入 State，作为 Controller 的参考轨迹。
                int state_ret = state->rsUpdateReferenceTrajectory(true, point);
                if (state_ret < 0) {
                    std::cerr << "Failed to update reference trajectory: "
                              << state_ret << std::endl;
                    return state_ret;
                }

                // Controller 根据当前 State 和参考轨迹计算下一周期关节命令。
                int controller_ret = controller->fcCalJointCommand(
                    *cmd, ik_eps, tune_eps);
                if (controller_ret < 0) {
                    std::cerr << "Failed to calculate joint command: "
                              << controller_ret << std::endl;
                    return controller_ret;
                }
            }
        }

        if (ret == 40002 || ret == 40003) {
            break;
        }
        if (ret < 0) {
            std::cerr << "Trajectory update failed: " << ret
                      << ", planner: " << failed_planner << std::endl;
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return ret;
        }
    }

    return 0;
}

CreateARALScene() 的声明位置和 Scene 生命周期管理方式以实际集成工程为准。示例重点是关节同步避障所需的对象关系、路点组织和接口调用顺序。

4. 双机械臂适配

双机械臂同样支持关节同步避障，调用流程与外部轴 + 机械臂一致。将两台机械臂分别配置为 MASTER 和 SLAVE，并在每个同步路点中分别填写两侧关节：

auto left_model_h = scene->rlCreateRobotModel(
    "left_arm", "/path/to/left_arm_model/");
auto right_model_h = scene->rlCreateRobotModel(
    "right_arm", "/path/to/right_arm_model/");

auto left_state_h = scene->rlCreateRobotState(left_model_h, "left_state");
auto right_state_h = scene->rlCreateRobotState(right_model_h, "right_state");

auto planner_h = scene->rlCreateRobotPlanner(
    {{left_model_h,  left_state_h,  PlannerRole::MASTER},
     {right_model_h, right_state_h, PlannerRole::SLAVE}},
    "dual_arm_joint_avoidance_planner");

PathPoint start;
start.id = 0;
start.type = DescribeSpace::JOINT;
start.joint_m = {0.20, 0.10, 0.80, 0.00, 1.20, 0.00};
start.joint_s = {0.10, 0.20, 0.70, 0.10, 1.10, 0.00};

PathPoint goal;
goal.id = 1;
goal.type = DescribeSpace::JOINT;
goal.joint_m = {0.60, -0.10, 1.10, 0.20, 1.00, 0.30};
goal.joint_s = {0.50, -0.20, 1.00, 0.10, 0.90, 0.20};

MoveProperty move_property;
move_property.maxV = std::vector<double>(12, 2.0);
move_property.maxA = std::vector<double>(12, 6.0);
move_property.maxJ = std::vector<double>(12, 60.0);

其中：

• joint_m 对应主机械臂，joint_s 对应从机械臂；
• 运动约束按主机械臂关节在前、从机械臂关节在后的顺序填写，本例总维数为 12；
• 必须设置两台机械臂在世界坐标系中的实际基座位姿，确保双臂互碰检测使用正确的相对安装关系；
• 双机械臂关节同步避障已显式给出两侧关节，因此保持默认 IKStrategy::NONE 即可。

5. 常见问题

5.1 是否支持多个 Planner 分别避障？

支持。可以为每个机器人分别创建 Planner，并由各 Planner 分别调用 tpGenerateCollisionFreePoints() 生成各自的关节空间无碰路径。本文示例采用单 Planner 的 MASTER + SLAVE 组织方式，用于生成具有同步点对应关系的主、从关节路径；多 Planner 方案请按多机同步规划流程组织。

5.2 是否还需要调用 tskWaitSyncMove() / tskSyncMoveOn()？

本文示例只有一个 Planner，主、从系统的两条关节空间无碰路径由规划结果建立点间对应关系并按统一时间进度执行，因此不需要调用这两个接口。采用多 Planner 方案时，可根据多机同步执行需求调用 tskWaitSyncMove() 和 tskSyncMoveOn()。

5.3 tpGenerateCollisionFreePoints() 成功后为何还要调用 tpAddPoints()？

前者只生成无碰的几何路点；后者根据 MoveProperty 对这些路点进行 B 样条路径规划和速度规划，生成可由 tskUpdateCycle() 周期采样的轨迹。

5.4 规划失败如何排查？

建议按以下顺序检查：

1. 主、从起点和终点是否分别在关节限位内且无碰；
2. 每个输入点是否同时填写正确维数的 joint_m、joint_s；
3. PathProperty::describe_space 和 PathPoint::type 是否均为 JOINT；
4. 两个模型的 mdlSetBasePoseInWorld() 是否与现场安装一致；
5. URDF/SRDF 的碰撞几何和允许碰撞配置是否正确；
6. 障碍物是否被重复注册或位姿坐标系使用错误；
7. 适当减小 maxDistance，或更换 RRT、LBKPIECE 进行对比；
8. 使用 scene->rlGetUtility()->rlGetErrorDescription(ret) 查询具体错误描述。

5.5 输出关节顺序是什么？

无碰路径点中：

• joint_m 对应创建 Planner 时标记为 MASTER 的模型；
• joint_s 对应标记为 SLAVE 的模型；
• 运动约束数组按 [master joints..., slave joints...] 排列。

任务周期输出参数 points 是 std::vector<TrajectoryPoint>。业务层不应假定points[0] 始终对应主侧、points[1] 始终对应从侧，而应根据每个TrajectoryPoint::id（模型句柄）识别其所属模型，并映射到对应的State、Controller。

6. 参考文档与接口

• 避障规划介绍及使用实例
• 多机同步规划说明
• ARAL Plan模块接口变更说明文档
• robot_scene.hpp
• robot_model.hpp
• robot_state.hpp
• robot_planner.hpp
• robot_task.hpp
• data_structure_definition.hpp